#pragma config(Sensor, in1,    TurnPot,        sensorPotentiometer)
#pragma config(Sensor, dgtl1,  ArmEncoderLeft, sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  ArmEncoderRight, sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  BaseEncoderLeft, sensorQuadEncoder)
#pragma config(Sensor, dgtl7,  BaseEncoderRight, sensorQuadEncoder)
#pragma config(Sensor, dgtl9,  SwitchToLargeGoal, sensorTouch)
#pragma config(Sensor, dgtl10, SwitchToSmallGoal, sensorTouch)
#pragma config(Sensor, dgtl11, SwitchVacume,   sensorTouch)
#pragma config(Sensor, dgtl12, AltButton,      sensorTouch)
#pragma config(Motor,  port1,           LeftBase,      tmotorNormal, openLoop)
#pragma config(Motor,  port2,           LeftArm,       tmotorNormal, openLoop)
#pragma config(Motor,  port5,           RightArm,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port6,           LeftVacume,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           RightVacume,   tmotorNormal, openLoop)
#pragma config(Motor,  port8,           LeftTurntable, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port9,           RightTurntable, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port10,          RightBase,     tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
//I like turtles
#pragma platform(VEX)


//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
// VACUUM SPEEDS
const int VacuumSlow = 60;
const int VacuumMedium = 70;
const int VacuumFast = 127;
// TURNTABLE SPEED
const int TurntableSpeed = 100;
const int TurntableSpeedMedium = 70;
const int TurntableSpeedSlow = 40;
// BASE SPEED
const int BaseSpeed = 127;
const int BaseSpeedMedium = 90;
const int BaseSpeedSlow = 50;
// DISTANCES
const int Counts = 8; // use Counts to multiply times the distance
const int ToSmallGoal = 55; //60 original and crappier lololololol
const int ToFirstScoringObjects = 45;
const int ToFirstScoringObjectsB = 75;
const int ToDoubler = 250; //300 original
const int ToDoublerPartTwo = 90; //40 original
const int ToOtherColoredTile = 150;

/////////////////////////
// TURNTABLE FUNCTIONS //
/////////////////////////

void TurntableOn(int Speed)
{
  motor(LeftTurntable) = Speed;
  motor(RightTurntable) = Speed;
}

void TurntableOff()
{
  motor(LeftTurntable) = 0;
  motor(RightTurntable) = 0;
}

void TurntableSmallGoalWinner()
{
  while (SensorValue[TurnPot] > 1000)
  {
    TurntableOn(127);
  }
  TurntableOff();
}

void TurntableBallAwesomesauce()
{
  while (SensorValue[TurnPot] < 3333)
  {
    TurntableOn(-127);
  }
  TurntableOff();
}

////////////////////
// BASE FUNCTIONS //
////////////////////
void ResetBaseEncoders()
{
  SensorValue(BaseEncoderLeft) = 0;
  SensorValue(BaseEncoderRight) = 0;
}

void DriveMotorStop()
{
  motor[LeftBase] = 0;
  motor[RightBase] = 0;
}

void MoveStraightDoubler(int distance, int speed)
{
   // set an integer value to subtract from the full speed for the side that is moving faster (for straightening)
   int speedOffset = (20);
	 ResetBaseEncoders();
	 distance = distance * Counts;
	 while (SensorValue(BaseEncoderRight) < distance)
	 {
	      //motor[LeftBase] = speed;
	      motor[RightBase] = speed - 40;
	  }
	 DriveMotorStop();
}

void MoveStraight(int distance, int speed)
{
   // set an integer value to subtract from the full speed for the side that is moving faster (for straightening)
   int speedOffset = (20);
	 ResetBaseEncoders();
	 distance = distance * Counts;
	 while (SensorValue(BaseEncoderRight) < distance)
	 {
	   int delta = abs(SensorValue(BaseEncoderLeft) - SensorValue(BaseEncoderRight));
	    if (SensorValue(BaseEncoderLeft) > SensorValue(BaseEncoderRight))//Left faster
	    {\
	      motor[LeftBase] = speed - speedOffset;
	      motor[RightBase] = speed;
	    }
	    if (SensorValue(BaseEncoderLeft) < SensorValue(BaseEncoderRight))//Right faster
	    {
	      motor[LeftBase] = speed;
	      motor[RightBase] = speed - speedOffset;
	    }
	    if (delta < 30)//Equal
	    {
	      motor[LeftBase] = speed - 3;
	      motor[RightBase] = speed;
	    }
	 }
	 DriveMotorStop();
}

void MoveStraightWithTurntable(int distance, int speed, int turdtable)
{
   // set an integer value to subtract from the full speed for the side that is moving faster (for straightening)
   int speedOffset = (20);
	 ResetBaseEncoders();
	 distance = distance * Counts;
	 bool moveturntable = true;

	 while (SensorValue(BaseEncoderRight) < distance)
	 {
	   int delta = abs(SensorValue(BaseEncoderLeft) - SensorValue(BaseEncoderRight));
	    if (SensorValue(BaseEncoderLeft) > SensorValue(BaseEncoderRight))//Left faster
	    {
	      motor[LeftBase] = speed - speedOffset;
	      motor[RightBase] = speed;
	    }
	    if (SensorValue(BaseEncoderLeft) < SensorValue(BaseEncoderRight))//Right faster
	    {
	      motor[LeftBase] = speed;
	      motor[RightBase] = speed - speedOffset;
	    }
	    if (delta < 30)//Equal
	    {
	      motor[LeftBase] = speed - 3;
	      motor[RightBase] = speed;
	    }
	    if (moveturntable)
	    {
        if (SensorValue[TurnPot] > turdtable)
        {
          TurntableOn(127);
        }
        else
        {
          TurntableOff();
          moveturntable = false;
        }
      }
	 }
	 DriveMotorStop();
}

void MoveBackwards(int distance, int speed)
{
   // set an integer value to subtract from the full speed for the side that is moving faster (for straightening)
   int speedOffset = 20;
	 distance = distance * Counts;
   ResetBaseEncoders();//Reset encoders with function, start at zero!
    {
    while (SensorValue(BaseEncoderRight) > distance)
    {
	    if (SensorValue(BaseEncoderLeft) > SensorValue(BaseEncoderRight))//Right faster
	    {
	      motor[LeftBase] = -(speed);
	      motor[RightBase] = -(speed - speedOffset);
	    }
	    if (SensorValue(BaseEncoderLeft) < SensorValue(BaseEncoderRight))//Left faster
	    {
	      motor[LeftBase] =  -(speed - speedOffset);
	      motor[RightBase] = -(speed);
	    }
	    if (SensorValue(BaseEncoderLeft) == SensorValue(BaseEncoderRight))//Equal
	    {
	      motor[LeftBase] = -(speed);
	      motor[RightBase] = -(speed);
	    }
    }
    DriveMotorStop();
  }
}


//////////////////////
// VACUUM FUNCTIONS //
//////////////////////
void VacuumOn(int Speed)
{
  motor(LeftVacume) = Speed;
  motor(RightVacume) = Speed;
}

void VacuumOff()
{
  motor(LeftVacume) = 0;
  motor(RightVacume) = 0;
}

////////////////////
// STEP FUNCTIONS //
////////////////////

// USER PRELOADS BATBOT WITH 3 PRELOADS

////////////////////////////////////////////////////////////////////////////////////////
// STEP ONE - PROGRAMMING SKILLS - SCORE THE THREE PRELOADS IN THE CLOSE 10 INCH GOAL //
////////////////////////////////////////////////////////////////////////////////////////
void StepOne()
{
  TurntableSmallGoalWinner();
  // MOVE STRAIGHT TO THE SMALL GOAL (THE TURNTABLE SHOULD HAVE ALREADY BEEN POSITIONED BY THE robot
  MoveStraight(ToSmallGoal, BaseSpeed - 10);
  // SPIT OUT OBJECTS INTO THE SMALL GOAL
  VacuumOn(VacuumSlow);
  wait1Msec(2000);
  VacuumOff();
  // MOVE BACKWARDS TO STARTING TILE
  MoveBackwards(-ToSmallGoal, BaseSpeed);
}

//////////////////////////////////////////////////////////////////////////////////////
// STEP TWO - PROGRAMMING SKILLS - GET THE FIRST TWO SCORING OBJECTS STRAIGHT AHEAD //
//////////////////////////////////////////////////////////////////////////////////////
// this ended up complicated - it failed to wedge the cylinder and ball and return reliably
// so we create a two step process:
// 1) User lines up the vacuum to the ball on top of the cylinder and presses the step button
// 2) Once Batbot returns with the ball in hand - press the step button again and the robot
//     lowers its' turntable just right - moves forward to the cylinder and then picks up the cylinder
//     at which point it returns back to starting tile - now ready to get the doubler
//////////////////////////////////////////////////////////////////////////////////////
void StepTwo()
{
  TurntableBallAwesomesauce();
  // MOVE STRAIGHT AHEAD TO STACKED SCORING OBJECTS
  VacuumOn(VacuumFast);
  MoveStraight(ToFirstScoringObjects, BaseSpeedSlow);
  // Will pick up the ball on top when it makes contact
  wait1Msec(200);
  VacuumOff();
  MoveBackwards(-10, BaseSpeedMedium);
  TurntableOn(TurntableSpeedSlow);
  wait1Msec(700);
  TurntableOff();
  VacuumOn(VacuumFast);
  MoveStraight(35, BaseSpeedMedium);
  wait1Msec(10);
  VacuumOff();
  /*MoveBackwards(-ToFirstScoringObjects, BaseSpeedSlow);
  // WAIT FOR STEP BUTTON TO BE PRESSED
  while(true)
  {
    if (SensorValue(SwitchToSmallGoal) == 1)
    {
      break;
    }
  }
  // MOVE THE TURNTABLE SLIGHTLY DOWN
  TurntableOn(TurntableSpeedSlow);
  wait1Msec(600);
  TurntableOff();
  VacuumOn(VacuumFast);
  // now move forward with vacumm off and turntable set at position
  MoveStraight(ToFirstScoringObjectsB, BaseSpeed);
  // we have reached the cylinder - turn the vacuum on at full power
  //VacuumOn(VacuumFast);
  wait1Msec(250); //wait1Msec(1500);
  VacuumOff();
  */
  // MOVE BACKWARDS TO STARTING TILE
  MoveBackwards(-ToFirstScoringObjects, BaseSpeed);
}

// USER ADJUST THE 2 OBJECTS SO THERE IS ROOM TO SUCK IN THE DOUBLER

///////////////////////////////////////////////////////////////////
// STEP THREE - PROGRAMMING SKILLS - DOUBLER SCORE TO SMALL GOAL //
///////////////////////////////////////////////////////////////////
void StepThree()
{
  // SHOULD HAVE TWO SCORING ITEMS ALREADY IN THE VACUUM
  //TurntableSmallGoalWinner();
  // MOVE FORWARD TO DOUBLER
  //MoveStraight(ToDoubler + ToDoublerPartTwo + 15, BaseSpeed);
  MoveStraightWithTurntable(ToDoubler + ToDoublerPartTwo + 15, BaseSpeed, 1000);
  VacuumOn(VacuumMedium);
  wait1Msec(3000);
  //MoveBackwards(ToDoublerPartTwo, BaseSpeed);
  TurntableOn(-TurntableSpeedSlow);
  wait1Msec(1500);
  TurntableOff();
  VacuumOn(VacuumFast);
  wait1Msec(1250);
  VacuumOff();
  //MoveStraight(ToDoublerPartTwo, BaseSpeed);
  TurntableSmallGoalWinner();
  //TurntableOn(TurntableSpeedMedium);
  //wait1Msec(1350);
  //TurntableOff();
  VacuumOn(VacuumMedium);
  wait1Msec(2000);
  // PUSH DOUBLE INTO SMALL GOAL
  // TURN VACUUM ON
  /*
  VacuumOn(VacuumFast);
  MoveStraight(ToDoublerPartTwo, BaseSpeed);
  //^^used to be BaseSpeedSlow
  wait1Msec(1250);
  VacuumOff();
  // MOVE TURNTABLE TOWARDS SMALL GOAL
  TurntableOn(TurntableSpeed);
  /////////////////
  // we need a timer here - if we hit the goal wrong we should not stall out - it will jsut sit there
  ////////////////
  wait1Msec(1200);
  TurntableOff();
  // TURN VACUUM BACK ON TO SPIT OUT SCORING OBJECTS
  VacuumOn(VacuumFast);
  wait1Msec(1000);
  VacuumOn(VacuumMedium);
  wait1Msec(2000);
  VacuumOff();
  // RETURN (MOVE STRIAGHT BACK) TO COLORED TILE
  */
  int moveBack = -(ToDoubler + ToDoublerPartTwo);
  MoveBackwards(moveBack, BaseSpeed);
}

//////////////////////////////////////////////////////////////////
// STEP FOUR - PROGRAMMING SKILLS - MOVE TO OTHER STARTING TILE //
//////////////////////////////////////////////////////////////////
void StepFour()
{
  MoveStraight(ToOtherColoredTile, BaseSpeed);
}
//////////////////////////////////
// STEP FIVE - SAME AS STEP ONE //
//////////////////////////////////
void StepFive()
{
  TurntableSmallGoalWinner();
  // MOVE STRAIGHT TO THE SMALL GOAL (THE TURNTABLE SHOULD HAVE ALREADY BEEN POSITIONED BY THE robot
  MoveStraight(ToSmallGoal, BaseSpeed - 10);
  // SPIT OUT OBJECTS INTO THE SMALL GOAL
  VacuumOn(VacuumMedium);
  wait1Msec(500);
  VacuumOn(VacuumFast);
  wait1Msec(2000);
  VacuumOff();
  // MOVE BACKWARDS TO STARTING TILE
  MoveBackwards(-ToSmallGoal, BaseSpeed);
}
/////////////////////////////////
// STEP SIX - SAME AS STEP TWO //
/////////////////////////////////
void StepSix()
{
  StepTwo();
}
/////////////////////////////////////
// STEP SEVEN - SAME AS STEP THREE //
/////////////////////////////////////
void StepSeven()
{
  StepThree();
}
/////////////////////////////////////
// STEP EIGHT - IF WE HAVE TIME??? //
/////////////////////////////////////
void StepEight()
{
  StepFour();
}
/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// All activities that occur before the competition starts

  // RESET THE BASE ENCODERS TO ZERO
  ResetBaseEncoders();
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////
// AUTONOMOUS BUTTONS:                                  ///
//  TOP BUTTON - MOVE THE TURNTABLE OUT                 ///
//  TOP BUTTON WITH ALTH - MOVE TURNTABLE BACK IN       ///
//  MIDDLE BUTTON - RUN NEXT STEP                       ///
//  BOTTOM BUTTON - VACUUM SUCKS ITEMS IN (SPITS OUT)   ///
///////////////////////////////////////////////////////////
// BUTTON TRANSLATIONS:                                 ///
// SwitchToLargeGoal = BOTTOM BUTTON = VACUUM           ///
// SwitchToSmallGoal = MIDDLE BUTTON = RUN STEP         ///
// SwitchVacume = TOP BUTTON = MOVE TURNTABLE           ///
///////////////////////////////////////////////////////////
task autonomous()
{
  // keep track of which step we are on
  int StepCount = 1;

  // begin autonomous loop
  while(true)
  {
    ////////// VACUUM BUTTONS //////////
	  if(SensorValue(SwitchToLargeGoal) == 1 && SensorValue(AltButton) == 0) // alt not pressed
	  {
	    // RUN THE VACUUM
	    VacuumOn(VacuumFast);
	  }
	  else if (SensorValue(SwitchToLargeGoal) == 1 && SensorValue(AltButton) == 1)
	  {
	    // reverse teh vacuum direction
	    VacuumOn(-VacuumFast);
	  }
	  else if (SensorValue(SwitchToLargeGoal) == 0)
	  {
	    // stop the vacuum
	    VacuumOff();
	  }

	  ////////// STEP BUTTONS //////////
	  if(SensorValue(SwitchToSmallGoal) == 1) // alt not pressed
	  {
	    // before each step reset encoders on base
	    ResetBaseEncoders();
	    // RUN NEXT STEP
	    switch (StepCount)
	    {
	      case 1:
	        // do step one
	        //StepOne();
	        StepCount++;
	        break;
	      case 2:
	        // do step two
	        //StepTwo();
	        StepCount++;
	        break;
	      case 3:
	        // do step three
	        //StepThree();
	        StepCount++;
	        break;
	      case 4:
	        //do step four
	        //StepFour();
	        StepCount++;
	        break;
	      case 5:
	        //do step five
	        StepFive();
	        StepCount++;
	        break;
	      case 6:
	        //do step six
	        StepSix();
	        StepCount++;
	        break;
	      case 7:
	        //do step seven
	        StepSeven();
	        StepCount++;
	        break;
	      case 8:
	        //you should know by now
	        StepEight();
	        StepCount++;
	        break;
	      default:
	        // we are done - do nothing
	    }
	  }

	  ////////// TURNTABLE BUTTONS //////////
	  if(SensorValue(SwitchVacume) == 1 && SensorValue(AltButton) == 0) // alt not pressed
	  {
	    // MOVE TURNTABLE OUT
	    TurntableOn(TurntableSpeed);
	  }
	  else if(SensorValue(SwitchVacume) == 1 && SensorValue(AltButton) == 1) // alt pressed
	  {
	    // MOVE TURNTABLE IN
	    TurntableOn(-TurntableSpeed);
	  }
	  else if (SensorValue(SwitchVacume) == 0)
	  {
	    // ??? IF NO TURNTABLE BUTTON PUSHED - DO WE NEED TO APPLY HOLD VALUE TO TURNTABLE
	    // we definitely need to turn it off
	    TurntableOff();
	  }

	}
}



/////////////////////////////////////////////////////////////////////////////////////////
//                                 User Control Task                                   //
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
  // NO USER CONTROL - ONLY USE THIS CODE FOR WORLD'S PROGAMMING SKILLS CHALLENGE

  // USE AUTONOMOUS BUTTONS TO RESET BATBOT BACK TO STARTING POSITION
}
